# -*- coding: utf-8 -*-
import os
import time
import re
import json
import math
import threading
from typing import List, Dict, Optional

try:
    import UE4CtrlAPI
    import UEMapServe
except ImportError:
    print("错误：UE4CtrlAPI.py 或 UEMapServe.py 未找到。请确保它们在PYTHONPATH中或与此脚本在同一目录。")
    exit(1)

ID_COUNTER_FILE = 'model_id_counter.txt'
DEFAULT_MAP_NAME = "Grasslands"
WEATHER_TYPES = {
    0: "晴天", 1: "少云", 2: "多云", 3: "阴", 4: "小雨", 5: "雨", 6: "暴雨", 7: "小雪", 8: "雪", 9: "暴雪", 10: "雾"
}

class ModelInfo:
    def __init__(self, model_id: int, model_type: int, position: List[float], angles: Optional[List[float]] = None):
        self.model_id = model_id
        self.model_type = model_type
        self.position = position
        self.angles = angles or [0.0, 0.0, 0.0]

class CharacterInfo(ModelInfo):
    def __init__(self, model_id: int, name: str, walking_type: int, standing_type: int,
                 position: List[float], patrol_points: List[List[float]], motion_logic: Dict,
                 angles: Optional[List[float]] = None):
        super().__init__(model_id, walking_type, position, angles)
        self.name = name
        self.walking_type = walking_type
        self.standing_type = standing_type
        self.patrol_points = patrol_points
        self.motion_logic = motion_logic

class MotionState:
    def __init__(self, current_pos, current_point_index, state, state_start_time, current_vehicle_type, current_yaw):
        self.current_pos = current_pos
        self.current_point_index = current_point_index
        self.state = state
        self.state_start_time = state_start_time
        self.current_vehicle_type = current_vehicle_type
        self.current_yaw = current_yaw

class SmoothTurnController:
    def __init__(self, turn_speed: float):
        self.target_yaw_rad = 0.0
        self.current_yaw_rad = 0.0
        self.turn_speed_rad_per_s = math.radians(turn_speed * 8)
    def update(self, target_rad: float, delta_time_s: float) -> float:
        current_yaw_norm = math.atan2(math.sin(self.current_yaw_rad), math.cos(self.current_yaw_rad))
        target_yaw_norm = math.atan2(math.sin(target_rad), math.cos(target_rad))
        angle_diff_rad = target_yaw_norm - current_yaw_norm
        if angle_diff_rad > math.pi:
            angle_diff_rad -= 2 * math.pi
        if angle_diff_rad < -math.pi:
            angle_diff_rad += 2 * math.pi
        max_turn_rad = self.turn_speed_rad_per_s * delta_time_s
        turn_amount_rad = max(min(angle_diff_rad, max_turn_rad), -max_turn_rad)
        self.current_yaw_rad += turn_amount_rad
        self.current_yaw_rad = self.current_yaw_rad % (2 * math.pi)
        return self.current_yaw_rad

class CharacterMotionManager:
    def __init__(self, ue, map_server):
        self.characters: List[CharacterInfo] = []
        self.states: Dict[int, MotionState] = {}
        self.yaw_controllers: Dict[int, SmoothTurnController] = {}
        self.ue = ue
        self.map_server = map_server
        self.running = False
        self.sim_interval = 1/20.0
        self.timer = None
    def add_character(self, char_info: CharacterInfo):
        self.characters.append(char_info)
        self.states[char_info.model_id] = MotionState(
            current_pos=list(char_info.position),
            current_point_index=0,
            state="moving",
            state_start_time=time.time(),
            current_vehicle_type=char_info.walking_type,
            current_yaw=0.0
        )
        self.yaw_controllers[char_info.model_id] = SmoothTurnController(
            turn_speed=char_info.motion_logic.get('turn_speed', 2.0)
        )
        print(f"已添加人物 {char_info.name} (ID: {char_info.model_id}) 到统一调度器")
    def update_all(self):
        now = time.time()
        for char in self.characters:
            state = self.states[char.model_id]
            logic = char.motion_logic
            speed = logic.get('speed', 1.5)
            turn_threshold = logic.get('turn_threshold', 0.3)
            showcase_duration = logic.get('showcase_duration', 2.0)
            patrol_points = char.patrol_points
            if not patrol_points:
                continue
            target_idx = (state.current_point_index + 1) % len(patrol_points)
            target_point = patrol_points[target_idx]
            if state.state == "moving":
                dx = target_point[0] - state.current_pos[0]
                dy = target_point[1] - state.current_pos[1]
                distance = math.sqrt(dx*dx + dy*dy)
                if distance < turn_threshold:
                    state.state = "showcasing"
                    state.state_start_time = now
                    state.current_vehicle_type = char.standing_type
                    state.current_point_index = target_idx
                else:
                    move_x = (dx / distance) * speed * self.sim_interval
                    move_y = (dy / distance) * speed * self.sim_interval
                    state.current_pos[0] += move_x
                    state.current_pos[1] += move_y
                    state.current_pos[2] = self.map_server.getTerrainAltData(state.current_pos[0], state.current_pos[1])
                    target_yaw_rad = math.atan2(dy, dx)
                    state.current_yaw = self.yaw_controllers[char.model_id].update(target_yaw_rad, self.sim_interval)
            elif state.state == "showcasing":
                if now - state.state_start_time >= showcase_duration:
                    state.state = "turning"
                    state.state_start_time = now
                    state.current_vehicle_type = char.walking_type
            elif state.state == "turning":
                next_target_idx = (state.current_point_index + 1) % len(patrol_points)
                next_target_point = patrol_points[next_target_idx]
                dx = next_target_point[0] - state.current_pos[0]
                dy = next_target_point[1] - state.current_pos[1]
                target_yaw_rad = math.atan2(dy, dx)
                state.current_yaw = self.yaw_controllers[char.model_id].update(target_yaw_rad, self.sim_interval)
                angle_diff_to_target = abs(target_yaw_rad - state.current_yaw)
                if angle_diff_to_target > math.pi:
                    angle_diff_to_target = 2 * math.pi - angle_diff_to_target
                if angle_diff_to_target < math.radians(5.0):
                    state.state = "moving"
            try:
                self.ue.sendUE4PosScale(
                    char.model_id,
                    state.current_vehicle_type,
                    0,
                    state.current_pos,
                    [0, 0, state.current_yaw],
                    [1, 1, 1]
                )
            except Exception as e:
                print(f"人物 {char.name} (ID: {char.model_id}) 位置发送失败: {str(e)}")
    def loop(self):
        if not self.running:
            return
        self.update_all()
        self.timer = threading.Timer(self.sim_interval, self.loop)
        self.timer.start()
    def start(self):
        if not self.running:
            self.running = True
            self.loop()
            print("统一人物运动调度器已启动")
    def stop(self):
        self.running = False
        if self.timer:
            self.timer.cancel()
        print("统一人物运动调度器已停止")

def parse_model_line(line: str) -> Optional[ModelInfo]:
    try:
        if not line.startswith('MODEL:'): return None
        parts = line.strip().split('|')
        model_id = int(parts[0].split(':')[1])
        model_type = int(parts[1].split(':')[1])
        pos_str = parts[2].split(':')[1]
        position = [float(x) for x in pos_str.split(',')]
        angles = [0.0,0.0,0.0]
        if len(parts) > 3 and parts[3].startswith('ANGLE:'):
            angle_str = parts[3].split(':')[1]
            angles = [float(x) for x in angle_str.split(',')]
        return ModelInfo(model_id, model_type, position, angles)
    except Exception: return None

def parse_character_line(line: str) -> Optional[CharacterInfo]:
    try:
        if not line.startswith('CHARACTER:'):
            return None
        parts = line.strip().split('|')
        if len(parts) < 7:
            print(f"警告：人物记录格式不完整: {line}")
            return None
        model_id = int(parts[0].split(':')[1])
        name = parts[1].split(':')[1]
        walking_type = int(parts[2].split(':')[1])
        standing_type = int(parts[3].split(':')[1])
        pos_str = parts[4].split(':')[1]
        position = [float(x) for x in pos_str.split(',')]
        patrol_points = []
        motion_logic = {}
        for part in parts[5:]:
            if part.startswith('PATROL_POINTS:'):
                points_str = part.split('PATROL_POINTS:')[1]
                patrol_points = [[float(x) for x in pt.split(',')] for pt in points_str.split(';')]
            elif part.startswith('MOTION_LOGIC:'):
                try:
                    motion_logic = json.loads(part.split('MOTION_LOGIC:')[1])
                except json.JSONDecodeError as e:
                    print(f"警告：运动逻辑JSON解析失败: {e}")
                    motion_logic = {"type": "patrol", "speed": 1.5, "turn_threshold": 0.3, "showcase_duration": 2.0, "turn_speed": 3.0}
        if not patrol_points:
            print(f"警告：人物 {name} (ID: {model_id}) 没有巡逻点")
            return None
        if not motion_logic:
            print(f"警告：人物 {name} (ID: {model_id}) 没有运动逻辑，使用默认值")
            motion_logic = {"type": "patrol", "speed": 1.5, "turn_threshold": 0.3, "showcase_duration": 2.0, "turn_speed": 3.0}
        return CharacterInfo(
            model_id=model_id,
            name=name,
            walking_type=walking_type,
            standing_type=standing_type,
            position=position,
            patrol_points=patrol_points,
            motion_logic=motion_logic,
            angles=[0.0, 0.0, 0.0]
        )
    except Exception as e:
        print(f"解析人物记录时发生错误: {str(e)}\n记录内容: {line}")
        return None

def restore_scene_from_id_counter():
    if not os.path.exists(ID_COUNTER_FILE):
        print(f"错误：找不到ID计数器文件: {ID_COUNTER_FILE}")
        return None, None, None
    with open(ID_COUNTER_FILE, 'r') as f:
        first_line = f.readline().strip()
    map_name, weather, time_val = 'Grasslands', '晴天', '1200'
    m = re.search(r'MapName:(\w+)', first_line)
    if m: map_name = m.group(1)
    m = re.search(r'Weather:([^,]+)', first_line)
    if m: weather = m.group(1)
    m = re.search(r'Time:(\d+)', first_line)
    if m: time_val = m.group(1)
    print(f"恢复场景：地图={map_name}，天气={weather}，天时={time_val}")
    return map_name, weather, time_val

def main():
    ue = UE4CtrlAPI.UE4CtrlAPI()
    map_server = UEMapServe.UEMapServe(DEFAULT_MAP_NAME)
    map_name, weather, time_val = restore_scene_from_id_counter()
    if map_name is None:
        return
    # 切换地图
    ue.sendUE4Cmd(f'RflyChangeMapbyName {map_name}')
    # 切换天气
    weather_id = 0
    for k, v in WEATHER_TYPES.items():
        if v == weather:
            weather_id = k
            break
    ue.sendUE4PosNew(100,804,[0,0,-8],[0,0,0],[0,0,0],[0,0,0,0,0,0,0,0])
    time.sleep(0.5)
    ue.sendUE4ExtAct(100, [weather_id, int(time_val)] + [0]*14)
    # 读取所有模型和人物
    static_models = []
    character_models_info = []
    with open(ID_COUNTER_FILE, 'r') as f:
        for line in f:
            line = line.strip()
            if not line or line.startswith('MapName:'):
                continue
            model = parse_model_line(line)
            if model:
                static_models.append(model)
                continue
            character = parse_character_line(line)
            if character:
                character_models_info.append(character)
    print(f"找到 {len(static_models)} 个静态模型和 {len(character_models_info)} 个人物模型。")
    # 部署静态模型
    for model in static_models:
        try:
            ue.sendUE4Pos(model.model_id, model.model_type, 0, model.position, model.angles)
            print(f"已部署静态模型 ID: {model.model_id}, 类型: {model.model_type} @ {model.position}")
        except Exception as e:
            print(f"部署静态模型 {model.model_id} 失败: {str(e)}")
    # 部署人物并加入调度器
    char_motion_mgr = CharacterMotionManager(ue, map_server)
    for char_info in character_models_info:
        try:
            ue.sendUE4Pos(char_info.model_id, char_info.walking_type, 0, char_info.position, char_info.angles or [0,0,0])
            print(f"已部署人物 {char_info.name} (ID: {char_info.model_id}) @ {char_info.position}")
            if char_info.patrol_points and char_info.motion_logic:
                char_motion_mgr.add_character(char_info)
            else:
                print(f"人物 {char_info.name} (ID: {char_info.model_id}) 无巡逻点或运动逻辑，仅放置。")
        except Exception as e:
            print(f"部署或启动人物 {char_info.name} (ID: {char_info.model_id}) 失败: {str(e)}")
    char_motion_mgr.start()
    print("场景恢复与人物运动已全部启动。按Ctrl+C可终止。")
    try:
        while True:
            time.sleep(1)
    except KeyboardInterrupt:
        char_motion_mgr.stop()
        print("已终止。");

if __name__ == '__main__':
    main() 